/* 
 * File:   ConcaveHull.h
 * Author: alrojas
 *
 * Created on 31 de marzo de 2014, 11:45
 */

#ifndef CONCAVEHULL_H
#define	CONCAVEHULL_H

#include "utils.h"

RNG rng1(1);

class ConcaveHull {
public:
    
    struct DistancePoint{

        Point point;
        double distance;

        DistancePoint(){};

        DistancePoint(Point point, double distance){
            this->point = point;
            this->distance = distance;
        }

    };

    struct AnglePoint{

        Point point;
        double angle;

        AnglePoint(){};

        AnglePoint(Point point, double angle){
            this->point = point;
            this->angle = angle;
        }

    };

    struct myclass {

        bool operator() (Point pt1,Point pt2) { return (pt1.y < pt2.y);}
        bool operator() (DistancePoint pt1,DistancePoint pt2) { return (pt1.distance < pt2.distance);}
        bool operator() (AnglePoint pt1,AnglePoint pt2) { return (pt1.angle > pt2.angle);}

    } myobject;
    
    struct myclass2 {

        bool operator() (Point pt1,Point pt2) { return (pt1.x < pt2.x);}

    } myobject2;
    
    
    
    ConcaveHull(){};
    
    void FindMinYPoint(vector < Point > & input, Point & output){

        vector < Point > tinput = input;
        
        std::sort(tinput.begin(),tinput.end(),myobject);
        output = tinput[0];

    }
    
    void FindBottomLeftPoint(vector < Point > & input, Point & output, int k){

        vector < Point > yinput = input;
        
        std::sort(yinput.begin(),yinput.end(),myobject);
        
        vector < Point > xinput;
        
        for(uint i = 0; i < k; i++){
            xinput.push_back(yinput[i]);
        }

        std::sort(xinput.begin(),xinput.end(),myobject2);
        
        output = xinput[0];

    }
    
    void RemovePoint(vector < Point > & input, Point & point){

        for(uint i = 0; i < input.size(); i++){
            if(input[i] == point){
                input.erase(input.begin() + i);
                return;
            }
        }

    }
    
    void KNearestNeighbours(Point & point, vector < Point > & input, int k,vector < Point > & output){
        
        vector < Point > linput = input;
        
        RemovePoint(linput,point);

        vector < Point > loutput;
        
        vector < DistancePoint > points(linput.size());
        for(uint i = 0; i < linput.size(); i++){
            points[i] = DistancePoint(linput[i],utils::EuclideanDistance(point,linput[i]));
        }

        std::sort(points.begin(),points.end(),myobject);

        for(int j = 0; j < k; j++){
            loutput.push_back(points[j].point);
        }
        
        output = loutput;

    }
    
    /*
    double AngleBetweenThreePoints(Point & first, Point & pivot, Point & second){

        Point a(first.x - pivot.x,first.y - pivot.y);
        Point b(second.x - pivot.x,second.y - pivot.y);

        double angle = acos((a.x * b.x + a.y * b.y) / (utils::EuclideanDistance(pivot,first) * utils::EuclideanDistance(pivot,second)));

        return angle * 180.0f / M_PI; 

    }
    */
    
    double AngleBetweenThreePoints(Point first, Point pivot, Point second){
    
        complex <double> P1(first.x,first.y),P2(pivot.x,pivot.y),P3(second.x,second.y);
    
        complex <double> P = (P1-P2)/(P3-P2);
    
        double angle = atan2(P.imag(),P.real());
    
        angle = angle * 180.0f / M_PI;
    
        if(angle < 0){
            angle += 360;
        }
    
        return angle;
    
    }
    
    void SortByAngle(Point & point, vector < Point > & input, vector < Point > & output, Point previous){

        vector < Point > loutput;
        
        vector < AnglePoint > points(input.size());
        for(uint i = 0; i < input.size(); i++){
            points[i] = AnglePoint(input[i],AngleBetweenThreePoints(previous,point,input[i]));
        }

        std::sort(points.begin(),points.end(),myobject);

        for(uint j = 0; j < points.size(); j++){
            loutput.push_back(points[j].point);
        }
        
        output = loutput;

    }
    
    bool intersection(Point o1, Point p1, Point o2, Point p2, Point &r){

        Point x = o2 - o1;
        Point d1 = p1 - o1;
        Point d2 = p2 - o2;

        float cross = d1.x*d2.y - d1.y*d2.x;
        if (abs(cross) < /*EPS*/1e-8)
            return false;

        double t1 = (x.x * d2.y - x.y * d2.x) / cross;
        r = o1 + d1 * t1;
        return true;
    }
    
    bool onSegment(Point & p, Point & q, Point & r){

        if (q.x <= max(p.x, r.x) && q.x >= min(p.x, r.x) &&
            q.y <= max(p.y, r.y) && q.y >= min(p.y, r.y))
           return true;

        return false;

    }
    
    // To find orientation of ordered triplet (p, q, r).
    // The function returns following values
    // 0 --> p, q and r are colinear
    // 1 --> Clockwise
    // 2 --> Counterclockwise
    int orientation(Point & p, Point & q, Point & r){

        int val = (q.y - p.y) * (r.x - q.x) -
                  (q.x - p.x) * (r.y - q.y);

        if (val == 0) return 0;  // colinear

        return (val > 0)? 1: 2; // clock or counterclock wise

    }
    
    bool IsThereIntersection(Point & p1, Point & q1, Point & p2, Point & q2){
        
        Point p;
        
        intersection(p1,q1,p2,q2,p);
        
        if(p1 == p) return false;
        if(q1 == p) return false;
        if(p2 == p) return false;        
        if(q2 == p) return false;
        
        // Find the four orientations needed for general and
        // special cases
        int o1 = orientation(p1,q1,p2);
        int o2 = orientation(p1,q1,q2);
        int o3 = orientation(p2,q2,p1);
        int o4 = orientation(p2,q2,q1);

        // General case
        if (o1 != o2 && o3 != o4)
            return true;

        // Special Cases
        // p1, q1 and p2 are colinear and p2 lies on segment p1q1
        if (o1 == 0 && onSegment(p1,p2,q1)) return true;

        // p1, q1 and p2 are colinear and q2 lies on segment p1q1
        if (o2 == 0 && onSegment(p1,q2,q1)) return true;

        // p2, q2 and p1 are colinear and p1 lies on segment p2q2
        if (o3 == 0 && onSegment(p2,p1,q2)) return true;

         // p2, q2 and q1 are colinear and q1 lies on segment p2q2
        if (o4 == 0 && onSegment(p2,q1,q2)) return true;

        return false; // Doesn't fall in any of the above cases

    }
    
    bool IsThereIntersection(vector < Point > & vinput, Point & p1, Point & q1){
        for(uint i = 1; i < vinput.size() - 1; i++){
            if(IsThereIntersection(vinput[i - 1],vinput[i],p1,q1)){
                return true;
            }
        }
        
        return false;
    }
    
    bool HullAllPoints(vector < Point > & vinput1, vector < Point > & vinput2){
        
        int counter = 0;
        
        for(uint i = 0; i < vinput2.size(); i++){
            for(uint j = 0; j < vinput1.size(); j++){
                if(vinput2[i] == vinput1[j]){
                    counter++;
                }
            }
        }
        
        return (counter == vinput2.size());
    }
    
    void convexHull(vector < Point > & vinput, vector < Point > & voutput){
        
        vector < vector < Point > > convexHulls(1);        
        cv::convexHull(Mat(vinput),convexHulls[0],false);
        
        voutput = convexHulls[0];
    }
    
    void concaveHull(vector < Point > & vinput, int iinput, vector < Point > & voutput){
        

        
        vector < Point > lvoutput;
        
        vector < Point > lvinput = vinput;
        
        int kk = max(iinput,3);
        Point firstPoint,currentPoint; 

        if(vinput.size() < 3){
            return;
        }

        if(vinput.size() == 3){
            voutput = vinput;
            return;
        }

        kk= min(kk,(int)vinput.size());
        
        FindMinYPoint(vinput,firstPoint);
        //FindBottomLeftPoint(vinput,firstPoint,kk);
//        FindMinYPoint(vinput,firstPoint);
        
        lvoutput.push_back(firstPoint);
        
        Point previousPoint(firstPoint.x - 200,firstPoint.y);
        currentPoint = firstPoint;
        
        //RemovePoint(vinput,currentPoint);
        
        int stage = 1;
        
        while((firstPoint != currentPoint || stage == 1) && vinput.size() > 0){
            
            stage = 2;
            
            vector < Point > kneighbours;
            KNearestNeighbours(currentPoint,lvinput,kk,kneighbours);
            SortByAngle(currentPoint,kneighbours,kneighbours,previousPoint);
            
            uint counter = 0;
            bool flag = false;
            while(!flag){
                while(counter < kneighbours.size() && !flag){

                    if(!IsThereIntersection(lvoutput,currentPoint,kneighbours[counter])){

                        previousPoint = currentPoint;
                        currentPoint = kneighbours[counter];
                        
                        lvoutput.push_back(currentPoint);
                        RemovePoint(lvinput,currentPoint);
                        
                        flag = true;
                    }
                    counter++;
                }
                if(!flag){
                    concaveHull(vinput,iinput + 1,voutput);
                    return;
           
                }
            }

        }
        
        if(!utils::AreAllPointsInside(vinput,lvoutput)){
            concaveHull(vinput,iinput + 1,voutput);
            return;
        }
        
        lvoutput.erase(lvoutput.end());

        voutput = lvoutput;
        
    }
    
    void DrawPolygon(Mat & minput, vector < Point > & vinput, int op){
        
        vector < Scalar > ccontour;
        vector < Scalar > cpoint;
        
        ccontour.push_back(cvScalar(0,0,255));
        ccontour.push_back(cvScalar(0,255,0));
        
        cpoint.push_back(cvScalar(0,255,255));
        cpoint.push_back(cvScalar(255,0,255));
        
        vector < vector < Point > > tvector;
        tvector.push_back(vinput);
        
        drawContours(minput,tvector,0,ccontour[op]);
        
        uint i;
        for(i = 0; i < vinput.size(); i++){
            circle(minput,vinput[i],2,cpoint[op],CV_FILLED);
            putText(minput,utils::Point2String(vinput[i],i + 1).c_str(),vinput[i],CV_FONT_HERSHEY_COMPLEX_SMALL,0.5,cvScalar(0,255,255));
        }  
        
    }
    
    void Test(){
        
        Mat background = Mat::zeros(600,600,CV_8UC3);
        
        vector < Point > points;
        
        for(uint i = 0; i < 20; i++){
            Point point(rng.uniform(50,550),rng.uniform(50,550));
            points.push_back(point);
            circle(background,point,2,cvScalar(255,255,255),CV_FILLED); 
            //putText(background,utils::Point2String(point).c_str(),point,CV_FONT_HERSHEY_COMPLEX_SMALL,0.5,cvScalar(0,255,255));
            cout << point << endl;
        }
        
        vector < vector < Point > > convexHulls(1);
        
        cv::convexHull(Mat(points),convexHulls[0],false);
        
        DrawPolygon(background,convexHulls[0],0);
        
        imwrite("Demo.JPG",background);
        
        vector < Point > concave_Hull;
        concaveHull(points,5,concave_Hull);
        cout << "Ended" << endl;
        
        DrawPolygon(background,concave_Hull,1);
        
        imshow("Concave Hull",background);
        cvWaitKey();        
        
    }
    
};

#endif	/* CONCAVEHULL_H */

